#include <robot/nao/cvirtualnao.h>

#include <buola/io/curi.h>
#include <buola/io/cresourcefinder.h>

#include <buola/scene/transform/crttransform.h>
#include <buola/scene/cgeode.h>
#include <buola/scene/transform/crevolutionjoint.h>

namespace buola { namespace robot { namespace nao {

CVirtualNao::CVirtualNao()
{
    mGroup=scene::CTransform::Import(io::data_finder().FindFile("robot/models/NAO/nao.vrml"));

    const char *lJointNames[]=
    {
        "HeadYaw","HeadPitch","LShoulderPitch","LShoulderRoll","LElbowYaw","LElbowRoll","LWristYaw","LPhalanx1","LHipYawPitch",
        "LHipRoll","LHipPitch","LKneePitch","LAnklePitch","LAnkleRoll","RHipYawPitch","RHipRoll","RHipPitch","RKneePitch",
        "RAnklePitch","RAnkleRoll","RShoulderPitch","RShoulderRoll","RElbowYaw","RElbowRoll","RWristYaw","RPhalanx1"
    };
    
    for(int i=0;i<26;i++)
    {
        mJoints[i]=find_revolution_joint(mGroup,lJointNames[i]);
    }

    for(int i=0;i<26;i++) mPose[i]=0;
}

CVirtualNao::~CVirtualNao()
{
}

void CVirtualNao::SetPose(const mat::CVecNd<26> &pPose)
{
    lock_guard<mutex> lLock(mMutex);
    mPose=pPose;
}

mat::CVecNd<26> CVirtualNao::GetPose()
{
    lock_guard<mutex> lLock(mMutex);
    return mPose;
}

void CVirtualNao::Update()
{
    lock_guard<mutex> lLock(mMutex);
    for(int i=0;i<26;i++)
        mJoints[i]->SetValue(mPose[i]);
}

/*namespace nao*/ } /*namespace robot*/ } /*namespace buola*/ }
